#include <cmath>
#include "Vector2D.hpp"

// -----------------------------------------------------------------------------
//                             PUBLIC METHODS
// -----------------------------------------------------------------------------

Vector2D::Vector2D()
{
  mX = mY = mMagnitude = 0.0d;
}

Vector2D::Vector2D(double x, double y)
{
  mX = x;
  mY = y;
  setMagnitude();
}

Vector2D::~Vector2D()
{

}

/**
 * @brief Overload of binary operator +
 * @brief Performs vector addition.
 * @param left 2D vector
 * @param right 2D vector
 */
const Vector2D Vector2D::operator+(const Vector2D &v) const
{
  return Vector2D(mX + v.mX, mY + v.mY);
}

/**
 * @brief Overload of binary operator -
 * @brief Performs vector substraction.
 * @param left 2D vector
 * @param right 2D vector
 */
const Vector2D Vector2D::operator-(const Vector2D &v) const
{
  return Vector2D(mX - v.mX, mY - v.mY);
}

/**
 * @brief Overload of binary operator *
 * @param left 2D vector
 * @param right 2D vector
 * @return Dot product
 */
double Vector2D::operator*(const Vector2D& v) const
{
  return (mX * v.mX) + (mY * v.mY);
}

/**
 * @brief Overload of binary operator *
 * @param left 2D vector
 * @param right k scaling factor
 * @return Scaled 2D vector
 */
const Vector2D Vector2D::operator*(double k) const
{
  return Vector2D(k * mX, k * mY);
}

/**
 * @brief Create a normalized vector.
 */
const Vector2D Vector2D::unit() const
{
  Vector2D targetVector;

  if (mMagnitude != 0.0d)
    {
      return Vector2D(mX / mMagnitude, mY / mMagnitude);
    }
  else
    {
      return Vector2D(0.0d, 0.0d);
    }
}

/**
 * @brief Create the normal of the calling vector object.
 */
const Vector2D Vector2D::normal() const
{
  return Vector2D(-mY, mX).unit();
}

/**
 * @brief Calculate the signed angle between two non-zero vectors.
 *        The calling vector object serves as the reference for the rotation.
 * @param v 2D vector
 * @return Angle in radians
 */
double Vector2D::getAngle(Vector2D &v) const
{
  // TODO: exception if left and/or right is zero vector.
  double angleSign;
  if (*this * v.normal() > 0) // Dot product
    {
      angleSign = -1.0f;
    }
  else
    {
      angleSign = 1.0f;
    }

  return std::acos((*this * v) / (mMagnitude * v.mMagnitude)) * angleSign;
}

// -----------------------------------------------------------------------------
//                             PRIVATE METHODS
// -----------------------------------------------------------------------------

void Vector2D::setMagnitude()
{
  mMagnitude = std::sqrt(mX * mX + mY * mY);
}

// -----------------------------------------------------------------------------
//                        RELATED NON-MEMBER METHODS
// -----------------------------------------------------------------------------

Vector2D operator*(double k, const Vector2D& v)
{
  return v * k;
}

std::ostream& operator<<(std::ostream& os, const Vector2D& v)
{
  os << "[ " << v.getX() << " " << v.getY() << " ]";
  return os;
}
